;#
;# basicops.S
;#
;# Basic Arithmetic Operations
;# 
;# Oded Rosen, 2011   
;#

	.text
	.globl L_mult
	.globl L_add
	.globl L_mac
	.globl L_shl
	.globl L_sub
	.globl L_msu
	.globl saturate
	.globl mult
	.globl add
	.globl sub
	.globl round
	.globl extract_h
	.globl extract_l
	.globl shr
	.globl L_add_c

	.arm
	.extern Overflow

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_mult                                                  |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   L_mult is the 32 bit result of the multiplication of var1 times var2    |
;#   with one shift left i.e.:                                               |
;#        L_mult(var1,var2) = L_shl((var1 times var2),1) and                   |
;#        L_mult(-32768,-32768) = 2147483647.                                |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_mult:
		;# r0, r1 are the multiply inputs.
		;# r0 is the result.
		SMULBB		r0,r0,r1		;# multiply 16x16 (r1,r2) into r1.
		QADD		r0,r0,r0
		BX			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_add                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   32 bits addition of the two 32 bits variables (L_var1+L_var2) with      |
;#   overflow control and saturation; the result is set at +2147483647 when  |
;#   overflow occurs or at -2147483648 when underflow occurs.                |
;#                                                                           |
;#   Complexity weight : 2                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#    L_var2   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_add:
		;# r0, r1 are the addition inputs.
		;# r0 is the result.
		qadd		r0,r0,r1	;# this is saturate add
		bx			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_mac                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Multiply var1 by var2 and shift the result left by 1. Add the 32 bit    |
;#   result to L_var3 with saturation, return a 32 bit result:               |
;#        L_mac(L_var3,var1,var2) = L_add(L_var3,L_mult(var1,var2)).         |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var3   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_mac:
		;# r1, r2 are the multiply inputs.
		;# r0 is the accumulator result 
		;# (also the input accumulator value).
		
		smulbb 		r1,r1,r2	;# multiply 16x16 (r1,r2) into r1.
		QDADD		r0,r0,r1
		bx			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : saturate                                                |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#    Limit the 32 bit input to the range of a 16 bit word.                  |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1                                                                 |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var1 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|

saturate:
		ssat		r0,#16,r0		;#if r0>0X00007fff:	r0=0X00007fff 
									;#if r0<0xffff8000:	r0=0xffff8000 
		bx			lr

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_shl                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Arithmetically shift the 32 bit input L_var1 left var2 positions. Zero  |
;#   fill the var2 LSB of the result. If var2 is negative, arithmetically    |
;#   shift L_var1 right by -var2 with sign extension. Saturate the result in |
;#   case of underflows or overflows.                                        |
;#                                                                           |
;#   Complexity weight : 2                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_shl:
		cmp 		r1,#0
		beq 		L_shl_end
		bmi			L_shl_shr  
		
		cmp 		r0,#0
		beq 		L_shl_end
		bmi			L_shl_neg
		clz 		r2,r0
		cmp 		r2,r1
		movle 		r0,#0x7fffffff
		ble			L_shl_Overflow
		lsl 		r0,r0,r1		
		b			L_shl_end
L_shl_shr:
		cmp 		r0,#0
		beq 		L_shl_end
		rsb			r1,r1,#0 
		asr 		r0,r0,r1
		b			L_shl_end
L_shl_neg:
		neg			r0,r0
		clz 		r2,r0
		cmp 		r2,r1
		movle 		r0,#0x80000000
		ble			L_shl_Overflow
		lsl			r0,r0,r1
		neg			r0,r0
		b			L_shl_end

L_shl_Overflow:
		ldr			r2,=Overflow
		mov			r1,#1
		strh		r1,[r2]

L_shl_end:
		bx			lr
		
;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : mult                                                    |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#    Performs the multiplication of var1 by var2 and gives a 16 bit result  |
;#    which is scaled i.e.:                                                  |
;#             mult(var1,var2) = extract_l(L_shr((var1 times var2),15)) and  |
;#             mult(-32768,-32768) = 32767.                                  |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|

mult:
		mul 		r0,r1,r0		
		asr 		r0,r0,#15		
;#		ssat 		r0,#16,r0	
		bx 			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_sub                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   32 bits subtraction of the two 32 bits variables (L_var1-L_var2) with   |
;#   overflow control and saturation; the result is set at +2147483647 when  |
;#   overflow occurs or at -2147483648 when underflow occurs.                |
;#                                                                           |
;#   Complexity weight : 2                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#    L_var2   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_sub:
		QSUB		r0,r0,r1 
		bx 			lr

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : add                                                     |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#    Performs the addition (var1+var2) with overflow control and saturation;|
;#    the 16 bit result is set at +32767 when overflow occurs or at -32768   |
;#    when underflow occurs.                                                 |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|

add:
		add 		r0,r0,r1	
		ssat 		r0,#16,r0
		bx 			lr

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : sub                                                     |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#    Performs the subtraction (var1+var2) with overflow control and satu-   |
;#    ration; the 16 bit result is set at +32767 when overflow occurs or at  |
;#    -32768 when underflow occurs.                                          |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|
	
sub:
		sub 		r0,r0,r1
		ssat 		r0,#16,r0
		bx 			lr

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_msu                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Multiply var1 by var2 and shift the result left by 1. Subtract the 32   |
;#   bit result to L_var3 with saturation, return a 32 bit result:           |
;#        L_msu(L_var3,var1,var2) = L_sub(L_var3,L_mult(var1,var2)).         |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var3   32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var3 <= 0x7fff ffff.                 |
;#                                                                           |
;#    var1                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#    var2                                                                   |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var1 <= 0x0000 7fff.                   |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    L_var_out                                                              |
;#             32 bit long signed integer (Word32) whose value falls in the  |
;#             range : 0x8000 0000 <= L_var_out <= 0x7fff ffff.              |
;#___________________________________________________________________________|

L_msu:
		smulbb 	r1,r1,r2
		QDSUB		r0,r0,r1
		bx			lr

;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : extract_h                                               |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Return the 16 MSB of L_var1.                                            |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1                                                                 |
;#             32 bit long signed integer (Word32 ) whose value falls in the |
;#             range : 0x8000 0000 <= L_var1 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|
 
extract_h:
		asr			r0,#16
		bx 			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : extract_l                                               |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Return the 16 LSB of L_var1.                                            |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1                                                                 |
;#             32 bit long signed integer (Word32 ) whose value falls in the |
;#             range : 0x8000 0000 <= L_var1 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|
								
extract_l:
		bx 			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : round                                                   |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#   Round the lower 16 bits of the 32 bit input number into the MS 16 bits  |
;#   with saturation. Shift the resulting bits right by 16 and return the 16 |
;#   bit number:                                                             |
;#               round(L_var1) = extract_h(L_add(L_var1,32768))              |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#   Inputs :                                                                |
;#                                                                           |
;#    L_var1                                                                 |
;#             32 bit long signed integer (Word32 ) whose value falls in the |
;#             range : 0x8000 0000 <= L_var1 <= 0x7fff ffff.                 |
;#                                                                           |
;#   Outputs :                                                               |
;#                                                                           |
;#    none                                                                   |
;#                                                                           |
;#   Return Value :                                                          |
;#                                                                           |
;#    var_out                                                                |
;#             16 bit short signed integer (Word16) whose value falls in the |
;#             range : 0xffff 8000 <= var_out <= 0x0000 7fff.                |
;#___________________________________________________________________________|

round:
		mov 		r1,#0x00008000
		adds 		r0,r0,r1			
		lsrvc 		r0,#16 
		bxvc 		lr 	
				
		movcs 		r0,#0x80000000 
		mvncc 		r0,#0x80000000
		lsr 		r0,#16  
		bx 			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : shr                                                   	 |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#  Arithmetically shift the 16 bit input var1 right var2 positions with	 |
;#   sign extension. If var2 is negative, arithmetically shift var1 left by  |
;#   -var2 with sign extension. Saturate the result in case of underflows or |
;#   overflows.																 |
;#                                                                           |
;#   Complexity weight : 1                                                   |
;#                                                                           |
;#___________________________________________________________________________|

shr:
		SSAT		r0,#16,r0
		CMP			r1,#0
		BEQ			shr_done
		BLT			shr_shl
		CMP			r1,#15
		BGE			shr_sgn
		ASR			r0,r0,r1	;# normal arithmatic shift right
		B			shr_done
shr_sgn:
		CMP			r0,#0
		MOVGE		r0,#0
		MOVLT		r0,#-1
		B			shr_done
shr_shl:
		CMP			r1,#-16
		MOVLT		r1,#-16
		QDSUB		r1,r1,r1
		LSL			r2,r0,r1
		SSAT		r3,#16,r2
		CMP			r2,r3
		BNE			shr_shl_of
		MOV			r0,r2
		B			shr_done
shr_shl_of:
		CMP			r0,#0
		LDRGT		r0,=32767
		LDRLE		r0,=-32768
shr_done:  
		BX 			lr


;#___________________________________________________________________________
;#                                                                           |
;#   Function Name : L_add_c                                                 |
;#                                                                           |
;#   Purpose :                                                               |
;#                                                                           |
;#  Performs 32 bits addition of the two 32 bits variables (L_var1+L_var2+C) |
;#   with carry. No saturation. Generate carry and Overflow values. The car- |
;#   ry and overflow values are binary variables which can be tested and as- |
;#   signed values.															 |
;#                                                                           |
;#___________________________________________________________________________|

L_add_c:
		ADD			r0,r0,r1
		BX 			lr
